#include "robot_manager.h"
#include "utilities/convert_data_type.hpp"
#include "utilities/file.hpp"
#include "utilities_function.h"

void RobotManager::commandMoveNullspaceCallback(const std_msgs::msg::Float64::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.float64 = msg;
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::MOVE_NULLSPACE, msg_);
    }
    cv_.notify_one();
}

void RobotManager::moveNullSpaceFunc(const CmdMsg &msg)
{
    ZLOGI("[Move Nullspace] Received command: %f", msg.float64->data);

    //! offline plan
    int ret = robot_algo_->rp_->moveNullspace(Vector2Eigen(joint_state_msg_.position),
                                              Eigen::VectorXd::Zero(robot_dof_), Eigen::VectorXd::Zero(robot_dof_),
                                              {msg.float64->data}, {0.0}, {0.0}, nullspace_planner_properties_);
    double plan_time = robot_algo_->rp_->getDuration();
    ZLOGI("plan ret: %d, traj_pos_size: %ld, during time: %f", ret, plan_time);

    rclcpp::Rate rate(CONTROL_FREQ);
    double t = 0.0;
    while (t < plan_time)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        pthread_mutex_lock(&zoros_mutex_);
        control(t);
        pthread_mutex_unlock(&zoros_mutex_);
        t += 1.0 / CONTROL_FREQ;
        rate.sleep();
    }
}

void RobotManager::commandMoveNullspaceWaypointCallback(const std_msgs::msg::String::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.str = msg;
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::MOVE_NULLSPACE_WAYPOINT, msg_);
    }
    cv_.notify_one();
}

void RobotManager::moveNullSpaceWaypointFunc(const CmdMsg &msg)
{
    std::istringstream ss(msg.str->data);
    ZLOGI("move nullspace waypoint rec data: %s", ss.str().c_str());
    std::vector<double> traj = ReadData1D(ss.str(), ',');
    std::cout << "traj" << Vector2Eigen(traj) << std::endl;

    //! offline plan
    std::vector<double> cartesian_position, cartesian_velocity, cartesian_acceleration;
    int points = traj.size();
    for (int i = 0; i < points; i++)
    {
        cartesian_position.push_back(traj[i]);
        cartesian_velocity.push_back(0.0);
        cartesian_acceleration.push_back(0.0);
    }

    int ret =
            robot_algo_->rp_->moveNullspace(Vector2Eigen(joint_state_msg_.position), Eigen::VectorXd::Zero(robot_dof_),
                                            Eigen::VectorXd::Zero(robot_dof_), cartesian_position, cartesian_velocity,
                                            cartesian_acceleration, nullspace_planner_properties_);
    double plan_time = robot_algo_->rp_->getDuration();
    ZLOGI("plan ret: %d, traj_pos_size: %ld, during time: %f", ret, plan_time);

    rclcpp::Rate rate(CONTROL_FREQ);
    double t = 0.0;
    while (t < plan_time)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        pthread_mutex_lock(&zoros_mutex_);
        control(t);
        pthread_mutex_unlock(&zoros_mutex_);
        t += 1.0 / CONTROL_FREQ;
        rate.sleep();
    }
}
